/*------------------------------------------------------------------------------------------*\

\*------------------------------------------------------------------------------------------*/

//#include <opencv2/core/core.hpp>
//#include <opencv2/highgui/highgui.hpp>
#include "VisualServo.hpp"
#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <stdlib.h>

#define P_SCALE 0.0254 //scale conversion of the object points to meter Size of 1 box in horizontal and vertical direction
//int main(int argc, char** argv[]) {

//	if(argc !=1){
//	printf("\n invalid number of argument e.g. ObjectRecognition img.png");
//	}

	double xV[4];
	double yV[4];
	double xoV[4];
	double yoV[4];
	int n_boards = 0;		//Number of snapshots of the chessboard
	int frame_step;			//Frames to be skipped
	int board_w;			//Enclosed corners horizontally on the chessboard
	int board_h;			//Enclosed corners vertically on the chessboard
	CvMat *intrinsic_matrix = (CvMat*)cvLoad("Intrinsics.xml");
	CvMat *distortion_coeffs = (CvMat*)cvLoad("Distortion.xml");
	//rotation vector and translation vector
	CvMat* rotation_vector  = cvCreateMat(3,1,CV_32FC1);
	CvMat* translation_vector = cvCreateMat(3,1,CV_32FC1);
//	CvMat* translation_vectors = cvCreateMat(30,1,CV_32FC1);

	int main() {
		CvCapture* capture;

	printf("Enter the numbers of spanspots = ");
	scanf("%d",&n_boards);

	printf("\rEnter the numbers of frames to skip = ");
	scanf("%d",&frame_step);

	board_w  = 6;
	board_h  = 6;

		int board_total  = board_w * board_h;						//Total enclosed corners on the board
		CvSize board_sz = cvSize( board_w, board_h );

		capture = cvCreateCameraCapture( 0 );
		if(!capture)
		{
			printf("\nCouldn't open the camera\n");
			return -1;
		}

		cvNamedWindow( "Snapshot" );
		cvNamedWindow( "Calibration");

		//Allocate storage for the parameters according to total number of corners and number of snapshots
			CvMat* image_points      = cvCreateMat(board_total,2,CV_32FC1);
			CvMat* object_points     = cvCreateMat(board_total,3,CV_32FC1);
			CvMat* point_counts      = cvCreateMat(n_boards,1,CV_32SC1);
//			CvMat* intrinsic_matrix  = cvCreateMat(3,3,CV_32FC1);
//			CvMat* distortion_coeffs = cvCreateMat(4,1,CV_32FC1);

			CvPoint2D32f* corners = new CvPoint2D32f[ board_total ];
				int corner_count;
				int successes = 0;
				int step, frame = 0;

				IplImage *image = cvQueryFrame( capture );
				IplImage *gray_image = cvCreateImage(cvGetSize(image),8,1);


				while(!successes)
					{
					cvShowImage( "Snapshot", gray_image );										//Show Gray if we didn't collect the image
						if((frame++ % frame_step) == 0)									//Skip frames
						{
							//Find chessboard corners:
							int found = cvFindChessboardCorners(image, board_sz, corners, &corner_count,CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS );

							cvCvtColor(image, gray_image, CV_BGR2GRAY);										//Get Subpixel accuracy on those corners
							cvFindCornerSubPix(gray_image, corners, corner_count, cvSize(13,13),cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
//why cvsize(11) and cvSize(-1,-1)
							cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);			//Draw it
							cvShowImage( "Calibration", image );
							// If we got a good board, add it to our data
							if( corner_count == board_total )
							{
								cvShowImage( "Snapshot", image );										//show in color if we did collect the image
								step = 0;
								for( int i=step, j=0; j<board_total; ++i,++j ) {
								CV_MAT_ELEM(*image_points, float,i,0) = corners[j].x;
								CV_MAT_ELEM(*image_points, float,i,1) = corners[j].y;
								CV_MAT_ELEM(*object_points,float,i,0) = (float) j/board_w;

								CV_MAT_ELEM(*object_points,float,i,1) = (float) (j%board_w);
								CV_MAT_ELEM(*object_points,float,i,2) = 0.0f;
								cvFindExtrinsicCameraParams2(object_points,image_points,intrinsic_matrix,distortion_coeffs,rotation_vector,translation_vector);

	//						CV_MAT_ELEM(*point_counts, int,successes,0) = board_total;
								for (int a=0;a<3;a++)
									{
								CV_MAT_ELEM(*translation_vector, float,a,0) = CV_MAT_ELEM(*translation_vector, float,a,0)/P_SCALE;

									}
									cvSave("rotation.xml",rotation_vector);
								cvSave("translation.xml",translation_vector);
								printf("\r%d successful Snapshots out of %d collected.",successes,n_boards);
								successes++;
							}
							}
							else
							successes=0;
						}

						image = cvQueryFrame( capture ); // Get next image



	}




				//corner points in image plane four border corner of the cass board
				xV[0]=CV_MAT_ELEM(*image_points,float,0,0);
				yV[0]=CV_MAT_ELEM(*image_points,float,0,1);
				xV[1]=CV_MAT_ELEM(*image_points,float,5,0);
				yV[1]=CV_MAT_ELEM(*image_points,float,5,1);
				xV[2]=CV_MAT_ELEM(*image_points,float,24,0);
				yV[2]=CV_MAT_ELEM(*image_points,float,24,1);
				xV[3]=CV_MAT_ELEM(*image_points,float,30,0);
				yV[3]=CV_MAT_ELEM(*image_points,float,30,1);




				//determine how indexing of the corner has been done and assign 4 corners of the edge as four points of the object
				xoV[0]=CV_MAT_ELEM(*object_points,float,0,0);
				yoV[0]=CV_MAT_ELEM(*object_points,float,0,1);
				xoV[1]=CV_MAT_ELEM(*object_points,float,5,0);
				yoV[1]=CV_MAT_ELEM(*object_points,float,5,1);
				xoV[2]=CV_MAT_ELEM(*object_points,float,24,0);
				yoV[2]=CV_MAT_ELEM(*object_points,float,24,1);
				xoV[3]=CV_MAT_ELEM(*object_points,float,30,0);
				yoV[3]=CV_MAT_ELEM(*object_points,float,30,1);

//Camera parameter from where the image has been taken
//translation
double tV[3];
double rV[3];
				tV[0]=CV_MAT_ELEM(*translation_vector, float,0,0);
				tV[1]=CV_MAT_ELEM(*translation_vector, float,1,0);
				tV[2]=CV_MAT_ELEM(*translation_vector, float,2,0);
//rotation theta 1,2 and 3
				rV[0]=CV_MAT_ELEM(*rotation_vector, float,0,0);
				rV[1]=CV_MAT_ELEM(*rotation_vector, float,1,0);
				rV[2]=CV_MAT_ELEM(*rotation_vector, float,2,0);
/*********************************Visual Servo**********************/

		Controller(xV,yV,xoV,yoV,tV,rV);



				cvReleaseMat( &object_points );
				cvReleaseMat( &image_points );
				cvReleaseMat( &point_counts );
				return 0;
	}



/*
	// read an image
	cv::Mat image= cv::imread("img.png");
	// create image window named "My Image"
	cv::namedWindow("My Image");
	// show the image on window
	cv::imshow("My Image", image);
	// wait key for 5000 ms
	cv::waitKey(5000);

	return 1;

}*/
